#include "pfsm_coord.h"
#include "axisgroup_robot.h"
#include "axisgroup.h"
#include "rob_common/robtool.h"
#include "rob_common/usrframe.h"
#include "zuc/tp/tpItf.hh"
#include "rob_common/robtool.h"
#include "rob_common/usrframe.h"
#include "log/zuclog.h"

#define MOTCOORD_INFO(fmt, ...) zuclog_info("MOTCOORD", fmt, ##__VA_ARGS__)
#define MOTCOORD_ERR(fmt, ...) zuclog_error("MOTCOORD", fmt, ##__VA_ARGS__)
#define MOTCOORD_DEBUG(fmt, ...) zuclog_debug("MOTCOORD", fmt, ##__VA_ARGS__)

axisgroup::PFsmCoord::PFsmCoord(Robot* grp) : PFsmState(grp) { tp_intf_ = TP::create_tp(); }

int axisgroup::PFsmCoord::enter()
{
    RobJointVal joint_pos_cmd = grp_->cmd_rst_.jpos;
    // MOTCOORD_DEBUG("joint_pos_cmd %s pos %s", joint_pos_cmd.val_str().c_str(), grp_->cmd_rst_.pose_str().c_str());
    tp_intf_->tpManager_tpSetCurJpos(joint_pos_cmd.val);
    tp_intf_->tpManager_tpSetPose(grp_->cmd_rst_.pose);
    tp_intf_->tpManager_tpSetPos_JPos(&grp_->cmd_rst_.pose, joint_pos_cmd.val);
    tp_intf_->tpManager_tpSetFilterJpos(joint_pos_cmd.val);
    tp_intf_->tpManager_tpclear();
    return 0;
}

int axisgroup::PFsmCoord::update_tp_constrain()
{
    // int max_fw_len;
    // int max_planning_queue_size;
    // double maxFeedScale;
    // PmHomogeneous o_to_world;
    // int percentage_mode_level;
    // int still_dec;
    // ZucPose disable_convyr_real_pos;
    // double actualTorque[ZUCMOT_MAX_JOINTS];

    // int stepping;

    // double joint_pos_fb[ZUCMOT_MAX_JOINTS];
    // double trq_limit[ZUCMOT_MAX_JOINTS];
    // double joint_pos_cmd[ZUCMOT_MAX_JOINTS];
    // double joint_veldir[ZUCMOT_MAX_JOINTS];
    // int IO_value[ZUCMOT_MAX_DIO];
    // int DO_value;
    // int AO_value;
    // double tcpVelLimit;
    // int safezone_Enable;
    // int safezone_Dengrous;
    // double elbow_sphere_radius;
    // int check_safezone_prediction;
    // double trajCycleTime;
    // ZucPose carte_pos_cmd;
    // ZucPose carte_pos_fb;
    // int inv_flag_7_dof;
    // int threePositionEnableLimit = 0;
    // int tcpForceLimit = 0;
    // int is_unlocked[ZUCMOT_MAX_JOINTS];
    // int is_track_pre_singular = 0;

    TP::TP_Status_In status_jkzuc_in_ = {};
    auto config = grp_->get_robot_active_config();
    status_jkzuc_in_.tool_offset = axisgroup::RobotToolMgr::get_rob_tool(grp_->tool_id_).pose();
    status_jkzuc_in_.base_offset = PmRpy{0, 0, 0};  // #TODO
    status_jkzuc_in_.user_offset = axisgroup::UsrFrameMgr::get_usr_frame(grp_->wobj_id_).world(grp_->base_frame_id_);

    status_jkzuc_in_.momvelscale = 1.0;   // 提供给缩减模式的倍率
    status_jkzuc_in_.maxFeedScale = 1.0;  //等于zucmotConfig->maxFeedScale
    status_jkzuc_in_.net_feed_scale = grp_->config_.rapid_scale;

    status_jkzuc_in_.enables_new = FS_ENABLED | SS_ENABLED | FH_ENABLED;
    status_jkzuc_in_.reduce_mode = 0;
    for (size_t i = 0; i < RobJointVal::MAX_AXIS; i++)
    {
        status_jkzuc_in_.vellimit_joint[i] = config.joint_limit.vel[i];
        status_jkzuc_in_.acclimit_joint[i] = config.joint_limit.acc[i];
        status_jkzuc_in_.j_pos_limit_up[i] = config.joint_limit.max[i];
        status_jkzuc_in_.j_pos_limit_low[i] = config.joint_limit.min[i];
        status_jkzuc_in_.joint_pos_fb[i] = grp_->get_moving_state().fdb.fdb_pos.val[i];
        status_jkzuc_in_.joint_pos_cmd[i] = grp_->cmd_rst_.jpos.val[i];

        if (i < 3)
        {
            status_jkzuc_in_.Vellimit_axes[i] = config.traj.vel.trans;
            status_jkzuc_in_.Acclimit_axes[i] = config.traj.acc.trans;
        }
        else
        {
            status_jkzuc_in_.Vellimit_axes[i] = config.traj.vel.ori;
            status_jkzuc_in_.Acclimit_axes[i] = config.traj.acc.ori;
        }
    }
    status_jkzuc_in_.last_pos_cmds = grp_->cmd_rst_.jpos;
    status_jkzuc_in_.carte_pos_cmd = grp_->cmd_rst_.pose;
    status_jkzuc_in_.inv_flag_7_dof = 1;
    //安全功能设置
    status_jkzuc_in_.safetyCons.stoppingDist = 1000;
    status_jkzuc_in_.safetyCons.stoppingTime = 1000;
    status_jkzuc_in_.safetyCons.tcpVelLimit = config.traj.vel.trans;
    status_jkzuc_in_.safetyCons.stoppingDistReduce = 1000;
    status_jkzuc_in_.safetyCons.stoppingTimeReduce = 1000;
    status_jkzuc_in_.safetyCons.enableLimitJointVel = 0;
    status_jkzuc_in_.safetyCons.tcpVelLimitReduce = config.traj.vel.trans;
    status_jkzuc_in_.safetyCons.elbowVelLimitReduce = config.traj.vel.trans;
    //转接参数设置
    // tp_intf_->status_in_.arc_blends_param = zucmotCommand->setuparcblends;
    tp_intf_->update_tp_status(status_jkzuc_in_);

    return 0;
}

int axisgroup::PFsmCoord::update_plan()
{
    update_tp_constrain();
    tp_intf_->tp_main_update();
    update_to_axisgroup();
    return 0;
}

int axisgroup::PFsmCoord::checkTpError()
{
    if (tp_intf_->tpManager_tpErrorID() == FAILED_FIND_FEASIBLE_SETS)
    {
        if (!finish_warn_failed)
        {
            tp_intf_->tpManager_tpabort();
            finish_warn_failed = 1;
            return -1;
        }
    }
    else if (tp_intf_->tpManager_tpErrorID() == CROSS_SAFETY_ZONE)
    {
        if (!finish_warn_cross_safearea)
        {
            tp_intf_->tpManager_tpabort();
            finish_warn_cross_safearea = 1;
            return -1;
        }
    }
    else
    {
        finish_warn_failed = 0;
        finish_warn_cross_safearea = 0;
    }
    return 0;
}

void axisgroup::PFsmCoord::abort() { tp_intf_->tpManager_tpabort(); }

bool axisgroup::PFsmCoord::is_plan_over() { return tp_intf_->tpManager_tpIsDone(); }

bool axisgroup::PFsmCoord::is_stopping() { return false; }

int axisgroup::PFsmCoord::init()
{
    TP::TP_Config_Stable tp_config_ = {};
    TP::TP_Status_In status_jkzuc_in_ = {};
    set_tp_config(tp_config_);
    // status_jkzuc_in_ #TODO
    tp_intf_->init(tp_config_, status_jkzuc_in_, grp_->kine_, grp_->dyna_);
    update_tp_constrain();
    return 0;
}

void axisgroup::PFsmCoord::pause()
{
    PFsmState::pause();
    tp_intf_->tpManager_tppause();
}

void axisgroup::PFsmCoord::resume()
{
    PFsmState::resume();
    tp_intf_->tpManager_tpresume();
}

bool axisgroup::PFsmCoord::is_paused() const { return is_paused_; }

int axisgroup::PFsmCoord::update_to_axisgroup()
{
    // update_tp_constrain();
    grp_->cmd_rst_.jpos = tp_intf_->status_out_.currentPos_joint;
    grp_->cmd_rst_.pose = tp_intf_->status_out_.carte_pos_desired;
    return 0;
}

int axisgroup::PFsmCoord::set_tp_config(TP::TP_Config_Stable& tp_config)
{
    auto config = grp_->get_robot_active_config();
    tp_config.cycleTime = grp_->config_.cyclictime_sec;
    tp_config._queueSize = DEFAULT_TC_QUEUE_SIZE_TOPPRA;
    tp_config.joint_num = grp_->joint_num();  //关节轴数设置
    tp_config.ini_vel_trans = config.traj.vel.trans;
    tp_config.ini_vel_ori = config.traj.vel.ori;
    tp_config.robot_serial_num = grp_->serial_num_;

    return 0;
}
